#! usr/bin/env python
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
#发布两个坐标系的位置关系
#一个是车辆底盘：base_link
#另一个是雷达：laser
#实现流程：
#导入包
#初始化节点
#创建发布对象
#组织发布数据
#发布数据
#spin
if __name__ == "__main__":
    rospy.init_node("static_pub_p")
    #创建发布对象
    pub = tf2_ros.StaticTransformBroadcaster()
    #组织发布的数据,设置头和偏移量以及四元数
    ts = TransformStamped()
    #header 
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = "base_link"
    #child frame
    ts.child_frame_id = "laser"
    #相对关系
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    #首先从欧拉角转化成四元数,这个函数的返回值就是四元数
    qtn = tf.transformations.quaternion_from_euler(1.57,1.57,1.57)
    #之后再设置四元数
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    pub.sendTransform(ts)
    rospy.spin()
